/*
 * PrimaryFollowTape.c
 */

#include "board.h"
#include "states.h"
#include "sensors.h"
#include "pwm.h"
#include "tick.h"

/* ************************************************************************** */
#define FOLLOW_TAPE_START 1
#define FOLLOW_TAPE_TRACK 2
#define FOLLOW_TAPE_DOUBLE_CHECK 3
#define FOLLOW_TAPE_FOUND_END 4
#define TAPE_END_SOURCE 1
#define TAPE_END_CIRCLE 2

/* ************************************************************************** */
void FollowTapeStart();
void FollowTapeTrack();
void FollowTapeDoubleCheck();
void FollowTapeFoundEnd();

int middleOn;
int leftOn;
int rightOn;
int foundTapeEndCount = 0;
int tapeDoubleCheckCount = 0;
int tapeDoubleCheckTries = 0;
int whichEndLookingFor = TAPE_END_SOURCE;

int currentFollowTapeState = FOLLOW_TAPE_START;
unsigned long int lastTapeUpdate = 0;

/*static const unsigned int kFullSpeed = 1000;*/
static const unsigned int kFullSpeed = 800;
static const unsigned int kSlowSpeed = 2500;
static const unsigned int kRightWheelOffset = 100;
static const unsigned int kMedSpeed = 2350;
static const unsigned int kUpdateTapeDelay = 15;
static const unsigned long int kDoubleCheckTime = 2;
static const int kCountMax = 10;
static const unsigned long int kStopTimeBeforeTurn = 1000;

/* ************************************************************************** */
void PrimaryStateFollowTape(void)
{
    middleOn = IsMiddlePhotoOnTape();
    leftOn = IsLeftPhotoOnTape();
    rightOn = IsRightPhotoOnTape();

    switch (currentFollowTapeState)
    {
        case FOLLOW_TAPE_START:
            FollowTapeStart();
            break;
        case FOLLOW_TAPE_TRACK:
            FollowTapeTrack();
            break;
        case FOLLOW_TAPE_DOUBLE_CHECK:
            FollowTapeDoubleCheck();
            break;
        case FOLLOW_TAPE_FOUND_END:
            FollowTapeFoundEnd();
            break;
        default:
            currentFollowTapeState = FOLLOW_TAPE_START;
    }
}

/* ************************************************************************** */
void FollowTapeStart()
{
    lastTapeUpdate = GetTick();

//    OC1R = kFullSpeed;
//    OC2R = kFullSpeed;
    foundTapeEndCount = 0;
    currentFollowTapeState = FOLLOW_TAPE_TRACK;
}

void FollowTapeTrack()
{
    /* Only update steering every 0.05 seconds*/
    if (GetTick() < lastTapeUpdate + kUpdateTapeDelay)
        return;
    lastTapeUpdate = GetTick();

    if (middleOn && !(leftOn || rightOn)) {
        OC1R = kFullSpeed - kRightWheelOffset;
        OC2R = kFullSpeed;
    }
    else if (leftOn && !rightOn) {
        OC1R = kFullSpeed - kRightWheelOffset;
        OC2R = kSlowSpeed;
    }
    else if (rightOn && !leftOn) {
        OC1R = kSlowSpeed - kRightWheelOffset;
        OC2R = kFullSpeed;
    }
    else if (rightOn && middleOn && leftOn) {
        foundTapeEndCount++;
        if (foundTapeEndCount > 1)
        {
            foundTapeEndCount = 0;
            OC1R = kMedSpeed - kRightWheelOffset;
            OC2R = kMedSpeed;
            currentFollowTapeState = FOLLOW_TAPE_DOUBLE_CHECK;
        }
    }
    else
    {
        foundTapeEndCount = 0;
    }
}

void FollowTapeDoubleCheck()
{
    if (GetTick() < lastTapeUpdate + kDoubleCheckTime)
            return;
    lastTapeUpdate = GetTick();

    tapeDoubleCheckTries++;
    if (rightOn && middleOn && leftOn)
        tapeDoubleCheckCount++;

    if (tapeDoubleCheckTries >= 10)
    {
        if (tapeDoubleCheckCount > 5)
        {
            Stop();
            currentFollowTapeState = FOLLOW_TAPE_FOUND_END;
        }
        else
        {
            OC1R = kFullSpeed - kRightWheelOffset;
            OC2R = kFullSpeed;
            Forward();
            currentFollowTapeState = FOLLOW_TAPE_TRACK;
        }
        tapeDoubleCheckTries = 0;
        tapeDoubleCheckCount = 0;
    }
}

void FollowTapeFoundEnd()
{
    if (GetTick() < lastTapeUpdate + kStopTimeBeforeTurn)
        return;
    lastTapeUpdate = GetTick();
    currentFollowTapeState = FOLLOW_TAPE_START;
    if (whichEndLookingFor == TAPE_END_SOURCE)
    {
        SetCurrentPrimaryState(PRIMARY_STATE_TURN_180);
        whichEndLookingFor = TAPE_END_CIRCLE;
    }
    else
        SetCurrentPrimaryState(PRIMARY_STATE_MIDDLE_CIRCLE);
}
